package com.sky.common.utils;

/**
 * pt计算工具类
 */
public class PtCalculator {


    /**
     * 计算2个物体的垂直角度
     *
     * @param groundDistance   摄像机与无人机的地面距离
     * @param cameraAltitude 摄像机海拔
     * @param uavAltitude    无人机海拔
     * @return 垂直夹角
     */
    public static double calVerticalDegrees(double groundDistance, double cameraAltitude, double uavAltitude) {
        double verticalDistance = uavAltitude - cameraAltitude;
        double verticalRadian = Math.atan(verticalDistance / groundDistance);
        return Math.toDegrees(verticalRadian);
    }


    /**
     * 计算ptz摄像机与无人机的水平和垂直夹角角度
     *
     * @param ptzLat      摄像机纬度
     * @param ptzLng      摄像机经度
     * @param ptzAltitude 摄像机海拔高度
     * @param uavLat      无人机纬度
     * @param uavLng      无人机经度
     * @param uavAltitude 无人机海拔高度
     * @return 水平角度:以北方为基准顺时针旋转,正东为90度，正西为270度。 垂直角度:向上为正，向下为负
     */
    public static double[] calculateAngle(double ptzLat, double ptzLng, double ptzAltitude, double uavLat, double uavLng, double uavAltitude) {
        //角度转换为弧度
        double ptzLatRadian = Math.toRadians(ptzLat);
        double ptzLngRadian = Math.toRadians(ptzLng);
        double uavLatRadian = Math.toRadians(uavLat);
        double uavLngRadian = Math.toRadians(uavLng);
        double lngDistance = uavLngRadian - ptzLngRadian;
        double y = Math.sin(lngDistance) * Math.cos(uavLatRadian);
        double x = Math.cos(ptzLatRadian) * Math.sin(uavLatRadian) - Math.sin(ptzLatRadian) * Math.cos(uavLatRadian) * Math.cos(lngDistance);
        double bearing = Math.atan2(y, x);
        // 将弧度转换为度
        bearing = Math.toDegrees(bearing);
        // 转换到-180到180之间
        if (bearing > 180) {
            bearing -= 360;
        }
        double verticalDistance = uavAltitude - ptzAltitude;
        double horDistance = GeoUtils.calculateDistance(ptzLat, ptzLng, uavLat, uavLng);
        double verticalRadian = Math.atan(verticalDistance / horDistance);
        double verticalDegrees = Math.toDegrees(verticalRadian);
        return new double[]{bearing, verticalDegrees};
    }

    /**
     * 计算两个水平角度之间的最小差值
     * @param angle1 第一个角度（-180到180）
     * @param angle2 第二个角度（-180到180）
     * @return 角度差值
     */
    public static double calHorizontalAngleDifference(double angle1, double angle2) {
        double difference = Math.abs(angle1 - angle2);
        // 如果差值大于180，取360减去差值
        if (difference > 180) {
            difference = 360 - difference;
        }
        return difference;
    }

    /**
     * 计算垂直角度差值
     * @param angle1 第1个角度
     * @param angle2 第2个角度
     * @return 垂直角度差值
     */
    public static double calVerticalAngleDifference(double angle1, double angle2) {
        return Math.abs(angle2-angle1);
    }



    public static void main(String[] args) {
        System.out.println(calVerticalDegrees(3, 100, 96));
//        double[] bearing;
//        bearing = calculateAngle(30.572599, 104.044938, 0, 30.594331, 104.045297, 0);
//        System.out.println("从摄像机到无人机的夹角：" + bearing[0] + " 度");
//        bearing = calculateAngle(30.572599, 104.044938, 0, 30.572575, 104.045953, 0);
//        System.out.println("从摄像机到无人机的夹角：" + bearing[0] + " 度");
//        bearing = calculateAngle(30.572599, 104.044938, 0, 30.554535, 104.045279, 0);
//        System.out.println("从摄像机到无人机的夹角：" + bearing[0] + " 度");
//        bearing = calculateAngle(30.572599, 104.044938, 0, 30.572851, 104.035631, 0);
//        System.out.println("从摄像机到无人机的夹角：" + bearing[0] + " 度");
    }
}